#ifndef _KALMAN_H_
#define _KALMAN_H_

#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

template <int V_X = 4, int V_Z = 2> //状态维数
class Kalman
{
    using Matrix_xxd = Matrix4f;
    using Matrix_x1d = Vector4f;

private:
    Matrix_x1d X; //状态变量
    Matrix_xxd A; //状态转移矩阵
    Matrix_xxd H; //观测矩阵
    Matrix_xxd P; //误差协方差矩阵
    Matrix_xxd Q; //过程噪声协方差矩阵
    Matrix_xxd R; //观测噪声协方差矩阵
    Matrix_xxd K; //卡尔曼增益
    float t;
    float last_t; //k-1

public:
    Kalman()=default;
    Kalman(Matrix_x1d X, Matrix_xxd A, Matrix_xxd H, Matrix_xxd P, Matrix_xxd Q, Matrix_xxd R, float time)
    {
        reset(X, A, H, P, Q, R, time);
    }
    void reset(Matrix_x1d X, Matrix_xxd A, Matrix_xxd H, Matrix_xxd P, Matrix_xxd Q, Matrix_xxd R, float time)
    {
        this->X = X;
        this->A = A;
        this->H = H;
        this->P = P;
        this->Q = Q;
        this->R = R;
        K = Matrix_xxd::Identity();
        last_t = time;
    }

    Matrix_xxd update(Matrix_xxd z_k, float time)
    {
        t = time;
        Matrix_xxd Z=z_k;
        for (int i = 0; i < V_Z; i++)
        {
            for (int j = V_Z; j < V_X; j++)
            {
                A(i, j) = t - last_t;
            }
        }
        X = A * X;
        P = A * P * A.transpose() + Q;
        K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
        X = X + K * (Z - H*X);
        last_t = t;
        P = (Matrix_xxd::Identity() - K * H) * P;
        return X;
    }
};                                            
#endif